!
! astrofit - fitting engine
!
!
! Copyright © 2011-2 F.Hroch (hroch@physics.muni.cz)
!
! This file is part of Munipack.
!
! Munipack is free software: you can redistribute it and/or modify
! it under the terms of the GNU General Public License as published by
! the Free Software Foundation, either version 3 of the License, or
! (at your option) any later version.
! 
! Munipack is distributed in the hope that it will be useful,
! but WITHOUT ANY WARRANTY; without even the implied warranty of
! MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
! GNU General Public License for more details.
! 
! You should have received a copy of the GNU General Public License
! along with Munipack.  If not, see <http://www.gnu.org/licenses/>.
!

module astrofitting
  
  implicit none
  
  integer, parameter, private :: dbl = selected_real_kind(15)
  real(dbl), parameter, private :: rad = 57.295779513082322865_dbl
  logical, private :: debug = .true.
  
contains


  subroutine astrometry(method,type,a,d,x,y,w,h,xc,yc,acen,dacen, &
       dcen,ddcen,sc,dsc,s0,rms,pa,dpa,ares,dres,id1_init,id2_init,id1_final,id2_final)

    use absfit
    use robfit
    use lsfit
    use astrotrafo
    use matcher
    use stat

    character(len=*), intent(in) :: method,type
    real(dbl), dimension(:), intent(in) :: a, d, x, y
    real(dbl), intent(in) :: w,h,xc,yc
    real(dbl), intent(out) :: acen,dcen,dacen,ddcen,sc,dsc,s0,rms,pa,dpa
    real(dbl), dimension(:), allocatable, intent(out) :: ares, dres
    integer, dimension(:), intent(in) :: id1_init,id2_init
    integer, dimension(:), allocatable, intent(out) :: id1_final,id2_final
    real(dbl), dimension(:), allocatable :: alpha,delta, xstar, ystar, u, v
    integer, dimension(:), allocatable :: id1, id2
    type(TypeProjection) :: t
    real(dbl) :: dlim,mad
    integer :: i,j,n

    ! check dimensions
    if( size(a) /= size(d) ) then
       write(*,*) 'astrometer: alpha and delta sizes does not corresponds.'
       stop 31
    end if

    if( size(x) /= size(y) ) then
       write(*,*) 'astrometer: xstar and ystar sizes does not corresponds.'
       stop 31
    end if

    ! check stars > 1 ???

    if( .not. (size(id1_init) > 1) ) then
       s0 = huge(s0)
       rms = huge(rms)
       return
    end if

    allocate(id1(size(id1_init)),id2(size(id2_init)))
    id1 = id1_init
    id2 = id2_init

    mad = -1.0

    ! rearrange matched objects
    n = size(id1)
    allocate(alpha(n),delta(n),xstar(n),ystar(n))
    do i = 1, n
       j = id1(i)
       alpha(i) = a(j)
       delta(i) = d(j)
!       if( debug ) write(*,*) alpha(i),delta(i)
    end do
    do i = 1, n
       j = id2(i)
       xstar(i) = x(j)
       ystar(i) = y(j)
!       if( debug ) write(*,*) xstar(i),ystar(i)
    end do

    ! initial robust estimation
    call absmin(type,alpha,delta,xc,yc,w,h,xstar,ystar,acen,dcen,sc,pa,rms,s0)

    ! estimator of MAD
    mad = 1.41*rms/0.6745
    if( debug ) write(*,*) '# mad=',mad,3600*mad

    if( .true. ) then
       ! select maximum count of stars as possible for final fit
       deallocate(alpha,delta,xstar,ystar)
       deallocate(id1,id2)
       n = size(a)
       allocate(id1(n),id2(n))
       dlim = max(1.0*mad,1.0e-6/3600.0_dbl)   ! WARNING: 1 mas  isn't portable !    
       call listmatch(type,a,d,x,y,acen,dcen,xc,yc,w,h,sc,pa,dlim,id1,id2)
       n = size(id1)
       allocate(alpha(n),delta(n),xstar(n),ystar(n),u(n),v(n))
       n = size(id1)
       do i = 1,n
          alpha(i) = a(id1(i))
          delta(i) = d(id1(i))
          xstar(i) = x(id2(i))
          ystar(i) = y(id2(i))
       end do
    else
       deallocate(alpha,delta,xstar,ystar)
       n = size(id1)
       allocate(alpha(n),delta(n),xstar(n),ystar(n),u(n),v(n))
       do i = 1,n
          alpha(i) = a(id1(i))
          delta(i) = d(id1(i))
          xstar(i) = x(id2(i))
          ystar(i) = y(id2(i))
       end do

    end if

    if( debug ) write(*,*) 'nstars=',n

    if( n > 5 ) then

       if( method == 'SQUARES' ) then
          call lsmin(type,alpha,delta,xc,yc,w,h,xstar,ystar,acen,dacen,dcen,ddcen,sc,dsc,pa,dpa,rms,s0)

       else ! if( method == 'ROBUST' ) then

          call robmin(type,alpha,delta,xc,yc,w,h,xstar,ystar,mad,acen,dacen,dcen,ddcen,sc,dsc,pa,dpa,rms,s0)
       end if
       
    else if( n > 1 ) then

       call lfit(type,alpha,delta,xstar,ystar, &
            xc,yc,acen,dacen,dcen,ddcen,sc,dsc,s0,rms,pa,dpa)
    else
 
       s0 = huge(s0)
       rms = huge(rms)
       return
       
!       write(*,*) "Minimum of two stars required."
!       stop 31

    end if

    t%type = type
    t%acen = acen
    t%dcen = dcen
    t%xcen = xc
    t%ycen = yc
    t%scale = sc
    t%pa = -pa

    ! residuals
    allocate(ares(n),dres(n))
    call invtrafo(t,xstar,ystar,u,v)
    ares = (u - alpha)/cos(dcen/rad)
    dres = (v - delta)

    allocate(id1_final(n),id2_final(n))
    id1_final = id1
    id2_final = id2

    deallocate(alpha,delta,xstar,ystar,u,v) 
    deallocate(id1,id2)

  end subroutine astrometry


! it would be better to derive uradius by using fwhm-like qunatity?
  

  ! low-precision estimator of uncertainties for a few data points
  subroutine lfit(type,alpha, delta, xstar, ystar, xcen, ycen, &
       acen,dacen,dcen,ddcen,sc,dsc,s0,rms,pa,dpa)

    !!!!!!    NO   TESTING   DONE  YET    !!!!
    
    use astrotrafo

    character(len=*), intent(in) :: type
    real(dbl),dimension(:), intent(in) :: alpha, delta, xstar, ystar
    real(dbl),intent(in) :: xcen,ycen
    real(dbl),intent(out) :: acen,dcen,dacen,ddcen,sc,dsc,s0,rms,pa,dpa
    real(dbl),dimension(:), allocatable :: u, v
    type(TypeProjection) :: t
    integer :: n
    real(dbl) :: tol,ds

    tol = sqrt(epsilon(tol))

    t%type = type
    t%xcen = xcen
    t%ycen = ycen
    t%scale = sc
    t%pa = pa
    t%acen = acen
    t%dcen = dcen

!    write(*,*) t%type,t%acen,t%dcen,t%xcen,t%ycen,t%scale,t%pa


    n = size(alpha)
    allocate(u(n),v(n))

    ! residual sum
    call trafo(t,alpha,delta,u,v)
    s0 = sum((xstar - u)**2 + (ystar - v)**2)
    deallocate(u,v) 

    ds = -1
    sc = t%scale
    dsc = ds
    rms = sqrt(s0/(size(alpha) - 2))
    dpa = 0
    dacen = 0
    ddcen = 0
!    pa = rad*p(1)
!    dpa = rad*dp(1)
!    dacen = dp(2)
!    ddcen = dp(3)

!    if( debug ) write(*,*) '# solution:',p
!    if( debug ) write(*,*) '# uncerts.: ',dp
    if( debug ) write(*,*) '# scale,dsc=',sc,dsc
    if( debug ) write(*,*) '# s0=',s0

  end subroutine lfit



  subroutine init(type,a,d,acen,dcen,scale)

    use astrotrafo
    use stat

    character(len=*), intent(in) :: type
    real(dbl), dimension(:), intent(in) :: a,d
    real(dbl), intent(out) :: acen,dcen,scale
    real(dbl), dimension(:), allocatable :: u,v,r
    type(TypeProjection) :: t
    integer :: n

    t%type = type
    t%xcen = 0.0
    t%ycen = 0.0
    t%scale = 1.0
    t%pa = 0.0

    ! estimate of center of field of view
    t%acen = median(a)
    t%dcen = median(d)
    if( debug ) write(*,*) '# initial acenter,dcenter=',t%acen,t%dcen
    
    n = size(a)
    allocate(u(n),v(n),r(n))

    ! initial projection, just only center in spherical coordinates is known
    call trafo(t,a,d,u,v)

    ! estimate effective size by 2-th moments
    r = sqrt(u**2 + v**2)
    scale = median(r)

    deallocate(r,u,v)

    acen = t%acen
    dcen = t%dcen

  end subroutine init


  subroutine listmatch(type,a,d,xx,yy,acen,dcen,xc,yc,w,h,scale,pa,tol,i1,i2)

    use astrotrafo
    use matcher

    character(len=*), intent(in) :: type
    real(dbl), dimension(:), intent(in) :: a,d,xx,yy
    real(dbl), intent(in) :: acen,dcen,xc,yc,w,h,scale,pa,tol
    integer, dimension(:), allocatable, intent(out) :: i1,i2
    real(dbl), dimension(:), allocatable :: x,y,u,v
    integer, dimension(:), allocatable :: id1,id2
    type(TypeProjection) :: t
    real(dbl) :: smin,c,s,sc0,x1,y1
    integer :: nsize,msize,n,i,j,idx

    if( size(a) /= size(d) .or. size(xx) /= size(yy) ) stop 666

    sc0 = (h + w)/2.0_dbl
    allocate(x(size(xx)),y(size(yy)))
    x = (xx - xc)/sc0
    y = (yy - yc)/sc0

    t%type = type
    t%acen = acen
    t%dcen = dcen
    t%xcen = 0.0_dbl
    t%ycen = 0.0_dbl
    t%scale = 1.0_dbl
    t%pa = 0.0_dbl

    ! select maximum amount of stars by list

    nsize = size(a)
    msize = size(x)
    allocate(u(nsize),v(nsize),id1(nsize),id2(nsize))

    call trafo(t,a,d,u,v)
    
!   stransform
    c = (sc0/scale)*cos(pa/rad)
    s = (sc0/scale)*sin(pa/rad)
    do i = 1, size(x)
       x1 = x(i)*c - y(i)*s
       y1 = x(i)*s + y(i)*c
       x(i) = x1
       y(i) = y1
!       write(*,*) x(i),y(i)
    end do

    ! remove duplicities
    call rmdup(x,y,msize,0.1*tol)
    call rmdup(u,v,nsize,0.1*tol)


    n = 0
    do i = 1, nsize
       smin = tol**2
       idx = 0  
       do j = 1, msize
          s = (u(i) - x(j))**2 + (v(i) - y(j))**2
          if( s < smin ) then
             ! the nearest and brighest star (on flux-sorted list)
             smin = s
             idx = j
          end if
       end do
       if( idx > 0 .and. .not. locate(idx,id2(1:n)) ) then
          n = n + 1
          id1(n) = i
          id2(n) = idx
          if( debug ) write(*,*) i,idx,u(i) - x(idx),v(i) - y(idx)
       end if
    end do    


!    allocate(r(nsize,msize))
!    forall( i = 1:nsize )
!       forall( j = 1:msize )
!          r(i,j) = sqrt((u(i) - x(j))**2 + (v(i) - y(j))**2)
!       end forall
!    end forall




    allocate(i1(n),i2(n))
    i1 = id1(1:n)
    i2 = id2(1:n)

    deallocate(id1,id2,u,v,x,y)

  end subroutine listmatch


  subroutine rmdup(x,y,n,tol)

    real(dbl), dimension(:), intent(inout) :: x,y
    integer, intent(inout) :: n
    real(dbl), intent(in) :: tol
    logical, dimension(:), allocatable :: id
    integer :: i,j

    allocate(id(size(x)))

    ! remove duplicities
    id = .false.
    do i = 1, size(x)
       do j = i + 1, size(x)
          if( sqrt((x(i) - x(j))**2 + (y(i) - y(j))**2) < tol ) then
             id(i) = .true.
             id(j) = .true.
!             write(*,'(2i5,4f10.5)') i,j,x(i),y(i),x(j),y(j)
          end if
       end do
    end do

!    write(*,*) id

!    do i = 1,size(x)
!       write(*,*) id(i),x(i),y(i)
!    end do

    n = 0
    do i = 1, size(x)
       
       if( .not. id(i) ) then
          n = n + 1
          x(n) = x(i)
          y(n) = y(i)
!          write(*,*) n,x(n),y(n),id(n)
       end if

    end do

    deallocate(id)

  end subroutine rmdup


end module astrofitting

